import RPi.GPIO as GPIO
import time

# set GPIO.BOARD Pins
# motor
Left_Black = 11  # IN1
Left_Red = 12  # IN2
Right_Black = 13  # IN3
Right_Red = 15   # IN4
# ultrasonic
GPIO_TRIGGER = 31  # pin6
GPIO_ECHO = 29   # pin5

MOTOR_STATE = None


def setup_gpio():
    GPIO.setwarnings(False)
    GPIO.setmode(GPIO.BOARD)
    # motor
    GPIO.setup(Left_Black, GPIO.OUT)
    GPIO.setup(Left_Red, GPIO.OUT)
    GPIO.setup(Right_Black, GPIO.OUT)
    GPIO.setup(Right_Red, GPIO.OUT)
    motor_stop()

    # ultrasonic
    GPIO.setup(GPIO_TRIGGER, GPIO.OUT)
    GPIO.setup(GPIO_ECHO, GPIO.IN)


def motor_turn_left():
    GPIO.output(Left_Red, False)
    GPIO.output(Left_Black, False)
    GPIO.output(Right_Red, GPIO.HIGH)
    GPIO.output(Right_Black, GPIO.LOW)
    MOTOR_STATE = "TURN_LEFT"


def motor_turn_right():
    GPIO.output(Left_Red, GPIO.HIGH)
    GPIO.output(Left_Black, GPIO.LOW)
    GPIO.output(Right_Red, False)
    GPIO.output(Right_Black, False)
    MOTOR_STATE = "TURN_RIGHT"


def motor_go():
    GPIO.output(Left_Red, GPIO.HIGH)
    GPIO.output(Left_Black, GPIO.LOW)
    GPIO.output(Right_Red, GPIO.HIGH)
    GPIO.output(Right_Black, GPIO.LOW)
    MOTOR_STATE = "GO"


def motor_stop():
    GPIO.output(Left_Red, GPIO.LOW)
    GPIO.output(Left_Black, GPIO.LOW)
    GPIO.output(Right_Red, GPIO.LOW)
    GPIO.output(Right_Black, GPIO.LOW)
    MOTOR_STATE = "STOP"


def motor_back():
    GPIO.output(Left_Red, GPIO.LOW)
    GPIO.output(Left_Black, GPIO.HIGH)
    GPIO.output(Right_Red, GPIO.LOW)
    GPIO.output(Right_Black, GPIO.HIGH)
    MOTOR_STATE = "BACK"


def distance():
    # set Trigger to HIGH
    GPIO.output(GPIO_TRIGGER, True)

    # set Trigger after 0.01ms to LOW
    time.sleep(0.00001)
    GPIO.output(GPIO_TRIGGER, False)

    StartTime = time.time()
    StopTime = time.time()

    # save StartTime
    while GPIO.input(GPIO_ECHO) == 0:
        StartTime = time.time()

    # save time of arrival
    while GPIO.input(GPIO_ECHO) == 1:
        StopTime = time.time()

    # time difference between start and arrival
    TimeElapsed = StopTime - StartTime
    # multiply with the sonic speed (34300 cm/s)
    # and divide by 2, because there and back
    distance = (TimeElapsed * 34300) / 2

    return distance


if __name__ == '__main__':
    setup_gpio()
    motor_go()
    try:
        while True:
            dist = distance()
            print ("Measured Distance = %.1f cm" % dist)
            if (dist < 25 and MOTOR_STATE != "STOP"):
                motor_stop()
            time.sleep(0.1)    
        # Reset by pressing CTRL + C
    except KeyboardInterrupt:
        print("stopped by User")
        GPIO.cleanup()
